geometry_msgs/PoseStamped origin
geometry_msgs/PoseStamped goal
geometry_msgs/Pose graspPose
